use core::arch::global_asm;

global_asm!(
    ".macro enable_dbg
    msr daifclr, #8
    .endm

    .macro mov_q, reg, val
    .if (((\\val) >> 31) == 0 || ((\\val) >> 31) == 0x1ffffffff)
    movz \\reg, :abs_g1_s:\\val
    .else
    .if (((\\val) >> 47) == 0 || ((\\val) >> 47) == 0x1ffff)
    movz \\reg, :abs_g2_s:\\val
    .else
    movz \\reg, :abs_g3:\\val
    movk \\reg, :abs_g2_nc:\\val
    .endif
    movk \\reg, :abs_g1_nc:\\val
    .endif
    movk \\reg, :abs_g0_nc:\\val
    .endm

    .macro reset_pmuserenr_el0, tmpreg
    mrs \\tmpreg, id_aa64dfr0_el1
    sbfx \\tmpreg, \\tmpreg, #8, #4
    cmp \\tmpreg, #1
    b.lt 9000f
    msr pmuserenr_el0, xzr
9000:
    .endm
.pushsection \".head.text\",\"ax\"
_head:
    b stext
    .long 0
.globl stext; .align 2; stext:
    mov x21, x0
    bl el2_setup
    bl __cpu_setup
    b __primary_switch
.type stext, @function; .size stext, .-stext
.globl el2_setup; .align 2; el2_setup:

    msr SPsel, #1
    mrs x0, CurrentEL
    cmp x0, #(2 << 2)
    b.eq 1f
    mov_q x0, (((((1) << (11))) | (((1) << (20))) | (((1) << (22))) | (((1) << (28))) | (((1) << (29)))) | 0)
    msr sctlr_el1, x0
    mov w0, #(0xe11)
    isb
    ret

    1: mov_q x0, (((((1) << (4))) | (((1) << (5))) | (((1) << (11))) | (((1) << (16))) | (((1) << (18))) | (((1) << (22))) | (((1) << (23))) | (((1) << (28))) | (((1) << (29)))) | 0)
    msr sctlr_el2, x0

    mov_q x0, (((((1) << (11))) | (((1) << (20))) | (((1) << (22))) | (((1) << (28))) | (((1) << (29)))) | 0)
    msr sctlr_el1, x0


    mov x0, #(0x00000040 | 0x00000080 | 0x00000100 | 0x00000200 | 0x00000005)

    msr spsr_el2, x0
    msr elr_el2, lr
    mov w0, #(0xe12)
    eret
.type el2_setup, @function; .size el2_setup, .-el2_setup
.globl __cpu_setup; .align 2; __cpu_setup:

    tlbi vmalle1
    dsb nsh

    mov x0, #3 << 20
    msr cpacr_el1, x0
    mov x0, #1 << 12
    msr mdscr_el1, x0
    isb
    enable_dbg
    reset_pmuserenr_el0 x0

    ldr x5, =((0x00) << ((0) * 8)) | ((0x04) << ((1) * 8)) | ((0x0c) << ((2) * 8)) | ((0x44) << ((3) * 8)) | ((0xff) << ((4) * 8)) | ((0xbb) << ((5) * 8))
    msr mair_el1, x5
    ret
.type __cpu_setup, @function; .size __cpu_setup, .-__cpu_setup
__primary_switch:
    adrp x4, __top_stack
    mov x1, x4
    mov sp, x1
    bl clear_bss
    b main
.type __primary_switch, @function; .size __primary_switch, .-__primary_switch
.popsection"
);

#[no_mangle]
fn clear_bss() {
    extern "C" {
        static __bss_start: usize;
        static __bss_stop: usize;
    }
    unsafe {
        let bss_start = &__bss_start as *const usize as *mut u8;
        let bss_end = &__bss_stop as *const usize as *mut u8;
        let count = bss_end as usize - bss_start as usize;
        core::intrinsics::write_bytes(bss_start, 0, count);
    }
}

#[export_name = "main"]
pub unsafe extern "C" fn main_trampoline(fdt: usize) {
    crate::main(fdt)
}
